
#include <gmtl/Matrix.h>
#include <gmtl/Generate.h>
#include <gmtl/Vec.h>

#include "skeleton.h"

#include <string>

void Skeleton::init() {
  
  // Skeleton joints
  char buffer[ 100];
  for ( int i = 0; i < MAX_JOINTS;i++){
    sprintf( buffer, "SkeletonJoint%d", i);
    _SkeletonJoint[ i].init( std::string( buffer));
  }
}


void Skeleton::update() {
  if ( _SkeletonJoint[0].isConnected()) {
    for ( int i = 0; i< MAX_JOINTS; i++) {
      _SkeletonJoint[ i]->updateData();
      _SkeletonMatrix[ i] = _SkeletonJoint[i]->getData( gadget::PositionUnitConversion::ConvertToMeters);
      gmtl::Point3f pos = gmtl::makeTrans<gmtl::Point3f>( _SkeletonMatrix[ i]);
      _Positions[ i] = pos;
    }
  }
}

